/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package luft27.navmath;

/**
 *
 * @author amalikov
 */
public class NavMath {
	public static final double LORAN_ERADIUS = 6378137.0 / 1852.0;
	public static final double NM_TO_M = 1852.0;
	public static final double RAD_TO_M = LORAN_ERADIUS * NM_TO_M;
	public static final double M_TO_RAD = 1.0 / RAD_TO_M;

	// приведение угла к диапазону (0..360)
	public static double wrapAngle360(double x) {
		if (x < 0.0)
			return x + 360.0;
		if (x >= 360.0)
			return x - 360.0;
		return x;
	}

	// приведение угла к диапазону (-180..180)
	public static double wrapAngle180(double x) {
		if (x > 180.0)
			return x - 360.0;
		if (x < -180.0)
			return x + 360.0;
		return x;
	}	
	
	// определение длины и истинного курса (в радианах) отрезка между заданными точками
	public static double[] getRngBrgGC_rad(double lat1, double lon1, double lat2, double lon2) {
		// Speranski
		double sa  = Math.sin(lon2 - lon1);
		double ca  = Math.cos(lon2 - lon1);
		double sf1 = Math.sin(lat1);
		double cf1 = Math.cos(lat1);
		double sf2 = Math.sin(lat2);
		double cf2 = Math.cos(lat2);

		double t = sf1*sf2 + cf1*cf2*ca;

		if (t >= 1)
			t = 0;
		else if (t <= -1)
			t = Math.PI;
		else
			t = Math.acos(t);

		double range = t;
		double brg = 0;
		if (range > 0.0000001)
			brg = Math.atan2(sa*cf2, cf1*sf2 - sf1*cf2*ca);

		return new double[]{range, brg};
	}

	// определение длины и истинного курса (в градусах) отрезка между 2-мя заданными точками
	public static double[] getRngBrgGC(double lat1, double lon1, double lat2, double lon2) {
		double res[] = getRngBrgGC_rad(Math.toRadians(lat1), Math.toRadians(lon1), Math.toRadians(lat2), Math.toRadians(lon2));
		res[0] *= RAD_TO_M;
		res[1] = Math.toDegrees(res[1]);
		return res;
	}

	// определение координат точки по заданным координатам начала отрезка, его длины и истинного курса (в радианах)
	public static double[] getPositionGC_rad(double lat1, double lon1, double range, double bearing) {
		double lat2 = Math.asin(Math.sin(lat1) * Math.cos(range) + Math.cos(lat1) * Math.sin(range) * Math.cos(bearing));
		double lon2 = (lon1 + Math.atan2(Math.sin(bearing) * Math.sin(range), Math.cos(lat1) * Math.cos(range) -
			Math.sin(lat1) * Math.sin(range) * Math.cos(bearing)));
		return new double[]{lat2, lon2};
	}

	// определение координат точки по заданным координатам начала отрезка, его длины и истинного курса (в градусах)
	public static double[] getPositionGC(double lat1, double lon1, double range, double bearing) {
		double[] res = getPositionGC_rad(Math.toRadians(lat1), Math.toRadians(lon1), range * M_TO_RAD, Math.toRadians(bearing));
		res[0] = Math.toDegrees(res[0]);
		res[1] = Math.toDegrees(res[1]);
		return res;
	}

	// определение бокового уклонения заданной точки от ЛЗП
	public static double getXteGC(double lat1, double lon1, double brg, double lat, double lon) {
		double range;
		double[] latlonC = getPositionGC(lat1, lon1, RAD_TO_M * Math.PI / 2.0, brg + 90.0);
		double[] rangeBrg = getRngBrgGC(latlonC[0], latlonC[1], lat, lon);
		rangeBrg[0] -= RAD_TO_M * Math.PI / 2.0;
		return -rangeBrg[0];
	}

	// определение длины и истинного курса (в радианах) отрезка между заданными точками
	public static double[] distanceCourse_rad(double lat1, double lon1, double lat2, double lon2) {
		double s_dlon = Math.sin(lon2 - lon1);
		double c_dlon = Math.cos(lon2 - lon1);
		double s_lat1 = Math.sin(lat1);
		double s_lat2 = Math.sin(lat2);
		double c_lat1 = Math.cos(lat1);
		double c_lat2 = Math.cos(lat2);

		double s_dlat2 = Math.sin((lat1 - lat2) / 2.0);
		double s_dlon2 = Math.sin((lon2 - lon1) / 2.0);

		double t = Math.sqrt(s_dlat2 * s_dlat2 + c_lat1 * c_lat2 * s_dlon2 * s_dlon2);

		if (t > 1.0)
			t = 1.0;

		if (t < -1.0)
			t = -1.0;	

		double dist = 2.0 * Math.asin(t);
		double crs = Math.atan2(s_dlon * c_lat2, c_lat1 * s_lat2 - s_lat1 * c_lat2 * c_dlon);
		return new double[]{dist, crs};
	}


	// определение длины и истинного курса (в градусах) отрезка между заданными точками
	public static double[] distanceCourse(double lat1, double lon1, double lat2, double lon2) {
		double res[] = distanceCourse_rad(Math.toRadians(lat1), Math.toRadians(lon1), Math.toRadians(lat2), Math.toRadians(lon2));
		res[0] *= RAD_TO_M;
		res[1] = Math.toDegrees(res[1]);
		return res;
	}


	// определение бокового удаления от ЛЗП и продольного удаления от ППМ
	public static double[] getTrackCoords(double lat1, double lon1, double trackCourse, double latD, double lonD) {
		double res[];
		double[] latlonC = getPositionGC(lat1, lon1, RAD_TO_M * Math.PI / 2.0, trackCourse + 90.0);
		res = getRngBrgGC(latlonC[0], latlonC[1], latD, lonD);
		double range = res[0];
		double trackErr = RAD_TO_M * Math.PI / 2.0 - range;

		res = distanceCourse(lat1, lon1, latD, lonD);
		double course1D = res[1];	// курс от конечной точки участка к текущей

		double atd = Math.acos(Math.cos(range * M_TO_RAD) / Math.cos(trackErr * M_TO_RAD)) * RAD_TO_M;
	
		/**
		* bugfix: если угол между ЛЗП и линией, соединяющей конечную точку и текущую, больше 90, то
		* продольное удаление от ППМ берётся со знаком минус
		*/
		double trackDist = (Math.abs(wrapAngle180(course1D - trackCourse)) < 90.0) ? atd : -atd;
		
		return new double[]{trackDist, trackErr};
	}
	
}
